/*
 * Copyright (c) 2011, Fredrik Häggström
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Luleå University of Technology nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
/**
 * \file
 * \brief The CAN Driver for LPC17xx.
 */

#include <tT.h>
#include <env.h>

#include "can.h"
#include "mcpwm.h"
#include "pid.h"



/* ************************************************************************** */

/**
 * \brief can_init Initializes the CAN module on LPC17xx
 * 
 * \param baudrate Pointer to the value of the wanted baudrate
 *
 * \return 0
 */
env_result_t can_init(can_t *self, uint32_t *baudrate)
{
	ENV_DEBUG("enter can_init()\r\n");
	
	uint32_t cykles, BRP = 0, i;
	uint8_t TSEG1 = 0,TSEG2 = 0, n;
	/* Power up the CAN1 unit */
	SC->PCONP |= (1<<13);
	
	/* Clock select 00 = CCLK/4 01 = CCLK 10 = CCLK/2 11 = CCLK/8	*/
	SC->PCLKSEL0 &= ~((3<<26)|(3<<28)|(3<<30));
	SC->PCLKSEL0 |= ((2<<26)|(2<<28)|(2<<30)); 	//CCLK/2
	
	
	PINCON->PINSEL0 |= (1<<0)|(1<<2);
	
	CAN1->MOD = 1; // Enter Reset Mode
	CAN1->IER = 0; // Disable All CAN Interrupts
	CAN1->GSR = 0;
	
	/* Request command to release Rx, Tx buffer and clear data overrun */
	//CANx->CMR = CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO;
	CAN1->CMR = (1<<1)|(1<<2)|(1<<3);
	/* Read to clear interrupt pending in interrupt capture register */
	CAN1->ICR;
	CAN1->MOD = 0;// Return Normal operating

	//Reset CANAF value
	CANAF->AFMR = 0x01;

	//clear ALUT RAM
	for (i = 0; i < 512; i++) {
		CANAF_RAM->mask[i] = 0x00000000;
	}

	CANAF->SFF_sa = 0x00;
	CANAF->SFF_GRP_sa = 0x00;
	CANAF->EFF_sa = 0x00;
	CANAF->EFF_GRP_sa = 0x00;
	CANAF->ENDofTable = 0x00;

	CANAF->AFMR = 0x02;		//Accept all messages
	
	/* (PCLK to CAN / baudrate ) */
	cykles = ( CCLK/2 )/ *baudrate;
	
	/* Calculate a value for the CAN Baudrate prescaler and 
	 * a suitable sample point 2/3ds in the bit
	 * n = nominal CAN bit time = (TSEG1 + TSEG2 + 3)
	 * n <= 24
	 * TSEG1 >= 2*TSEG2
	 */
	
	for(n=24; n>0; n=n-2)
	{
		if ( (cykles%n)==0 )
		{
			BRP = cykles / n - 1;
			TSEG2 = (n/3) - 1;
			TSEG1 = n -TSEG2 - 3;
			break;
		}
	}
	
	
	/* Enter reset mode */
	CAN1->MOD = 0x01;
	
	/* Set bit timing
	 * Default: SAM = 0x00;
	 *          SJW = 0x03;
	 */
	CAN1->BTR  = (TSEG2<<20) | (TSEG1<<16) | (3<<14) | BRP;
	
	/* Return to normal operating */
	CAN1->MOD = 0;
	CAN1->IER = (1<<2) | (1<<0); 	/* Enable CAN Error and Rx Interrupt */
	
	
	TT_ASYNC(self, can_keep_alive, TT_ARGS_NONE);
	
	
	NVIC_EnableIRQ(CAN_IRQn);
	
	NVIC_SetPriority(CAN_IRQn, 1);
	
	return 0;
}

env_result_t can_enable(can_t *self, void *none)
{
	ENV_DEBUG("can_enable()\r\n");
	CAN1->MOD = 1; // Enter Reset Mode
	CAN1->IER = 0; // Disable All CAN Interrupts
	CAN1->GSR = 0;
	
	/* Request command to release Rx, Tx buffer and clear data overrun */
	//CANx->CMR = CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO;
	CAN1->CMR = (1<<1)|(1<<2)|(1<<3);
	/* Read to clear interrupt pending in interrupt capture register */
	CAN1->ICR;
	CAN1->EWL = 0x60;
	CAN1->IER = (1<<2) | (1<<0); 	/* Enable CAN Error and Rx Interrupt */
	CAN1->MOD = 0;// Return Normal operating
	CAN1->RFS;
	//Reset CANAF value
	CANAF->AFMR = 0x00;
	CAN1->ICR;
	CAN1->CMR = (1<<1)|(1<<2)|(1<<3);
	return 0;
}

/* ************************************************************************** */

/**
 * \brief can_send Sends a CAN message
 * 
 * \param self Pointer to CAN object
 * \param can_msg	Pointer to CAN message that will be sent
 *
 * \return CAN_ERROR if failed
 */
env_result_t can_send(can_t *self, can_msg_t *can_msg)
{
  ENV_DEBUG("enter can_send()\r\n");
  //Check status of Transmit Buffer 1
  if (CAN1->SR & (1<<2))
  {
    /* Transmit Channel 1 is available */
    /* Write frame informations and frame data into its CANxTFI1,
    * CANxTID1, CANxTDA1, CANxTDB1 register */
    CAN1->TFI1 = ((can_msg->len & 0x0F) << 16) | ((can_msg->type & 0x01)<<30) | ((can_msg->format & 0x01)<<31) ;

    /* Write CAN ID*/
    CAN1->TID1 = can_msg->id;
    
   
    CAN1->TDA1 = can_msg->data[0];
    CAN1->TDB1 = can_msg->data[1];
    
    /*Write transmission request*/
    CAN1->CMR = (1<<0) | (1<<5);
    ENV_DEBUG("SENT!!\r\n");
    self->Tx++;
    return 0;
     
  }
  return CAN_ERROR;
}

void can_push_filter(can_t *self, uint32_t type)
{
	uint32_t i = 0, temp = 0, temp2 = 0;
	
	// Push all ext_grp one slot down and the ext_grp pointers one point up.
	if ( type == CAN_EXT || type == CAN_STD_GRP || type == CAN_STD ) { 
		temp = CANAF->EFF_GRP_sa / 4;
		temp2 = self->std + self->std_grp + self->ext ;
		for (i = self->ext_grp ; i > 0 ; i++) {
			
			CANAF_RAM->mask[temp + i] = CANAF_RAM->mask[temp + i - 1];
			
			CANAF_RAM->mask[511 - temp2 -i ] = CANAF_RAM->mask[511 - temp2 -i + 1];
		}
		CANAF->EFF_GRP_sa +=0x04;
	}
	// Push all ext one slot down and the ext pointers one point up.
	if ( type == CAN_STD_GRP || type == CAN_STD ) { 
		temp = CANAF->EFF_sa / 4;
		temp2 = self->std + self->std_grp  ;
		for (i = self->ext ; i > 0 ; i++) {
			
			CANAF_RAM->mask[temp + i] = CANAF_RAM->mask[temp + i - 1];
			
			CANAF_RAM->mask[511 - temp2 -i ] = CANAF_RAM->mask[511 - temp2 -i + 1];
		}
		CANAF->EFF_sa     +=0x04;
	}
	// Push all std_grp one slot down and the std_grp pointers one point up.
	if ( type == CAN_STD ) { 
		temp = CANAF->SFF_GRP_sa / 4;
		temp2 = self->std ;
		for (i = self->std_grp ; i > 0 ; i++) {
			
			CANAF_RAM->mask[temp + i] = CANAF_RAM->mask[temp + i - 1];
			
			CANAF_RAM->mask[511 - temp2 -i ] = CANAF_RAM->mask[511 - temp2 -i + 1];
		}
		CANAF->SFF_GRP_sa +=0x04;
	}
		
	CANAF->ENDofTable +=0x04;
}

/* ************************************************************************** */

/**
 * \brief can_installer Installs a listner in the Acc filter
 * 
 * \param self Pointer to CAN Object.
 * \param listner Pointer to installing listner
 *
 * \return CAN_ERROR if filter is full
 */
env_result_t can_installer(can_t *self, can_listner_t *listner) 
{
  uint32_t temp,i;
  
  ENV_DEBUG("enter can_installer()\r\n");
  
  env_send(listner->id&0x1FFFFFFF);
  ENV_DEBUG("\r\n");
  
  /* Acceptance Filter Memory full - return */
	temp =  ((self->Full_CAN + 1)>>1) + self->Full_CAN*3 \
				 + ((self->std + 1) >> 1) \
				 + self->std_grp \
				 + self->ext \
				 + (self->ext_grp<<1);
	if (temp >= 512){ //don't have enough space
		return 1;
	}
	
  /* Turn off filter so writing is possible */
  CANAF->AFMR = 0x01;
  
  
  if (listner->id & CAN_STD_ID_FORMAT){
  	ENV_DEBUG("STD\r\n");
  	for (i = 0 ; i < self->std; i++) {
  		
  		if ( !(i&0x01) && (CANAF_RAM->mask[i/2] & (0x07FF<<16)) == ((listner->id & 0x07FF)<<16)) {
  			if ( listner->index ) {
  				listner->next = (can_listner_t*)(CANAF_RAM->mask[511-i]);
  				CANAF_RAM->mask[511-i] = (uint32_t)(listner);
  				ENV_DEBUG("New entry to listner Queue 0\r\n");
  				break;
  			}
  			else {
  				ENV_DEBUG("Cant't queue listners for non 'can open' listners");
  				return CAN_ERROR;
  			
  			}
  		}
  		else if ( (CANAF_RAM->mask[i/2] & (0x07FF<<0))== ((listner->id & 0x07FF)<<0)) {
  			if ( listner->index ) {
					listner->next = (can_listner_t*)(CANAF_RAM->mask[511-i]);
					CANAF_RAM->mask[511-i] = (uint32_t)(listner);
					ENV_DEBUG("New entry to listner Queue 1\r\n");
					break;
				}
				else {
					ENV_DEBUG("Cant't queue listners for non 'can open' listners");
  				return CAN_ERROR;
				}
  		}
  	}
  	
  	if ( i == self->std ) { //The ID mask is not set yet
  		
  		ENV_DEBUG("New STD entry to Acc filter\r\n");
  		
  		
  		
  		if ( !(i & 0x01)) {
				
				env_send(CANAF_RAM->mask[511-i]);
				ENV_DEBUG("\r\n");
				// update the register pointers if a new row is filled
				can_push_filter(self,CAN_STD);
				
  			//CANAF_RAM->mask[i/2] = 0x0000FFFF | (((1<<13) | listner->id)  << 16);
  			CANAF_RAM->mask[i/2] = 0x0000FFFF | (((0<<13) | listner->id)  << 16);
  			CANAF_RAM->mask[511-i] = (uint32_t)(listner);
  			env_send(CANAF_RAM->mask[0]);
				ENV_DEBUG(" Filter ID\r\n");
				env_send((uint32_t)(listner));
				ENV_DEBUG("\r\n");
  			ENV_DEBUG("Add rows\r\n");
  			
  		}
  		else {
  			
  			CANAF_RAM->mask[i/2] &= 0xFFFF0000 | (((1<<13) | listner->id)  << 0);
  			CANAF_RAM->mask[511-i] = (uint32_t)(listner);
  			
  			
  		}
  		self->std++;
  	}
  }
  else {		// CAN_EXT_ID_FORMAT
  	ENV_DEBUG("EXT\r\n");
  	for (i = 0 ; i < self->ext; i++) {
  		
  		if ( (CANAF_RAM->mask[CANAF->EFF_sa / 4 + i] & 0x1FFFFFFF) == (listner->id & 0x1FFFFFFF) ){
  			if ( listner->index ) {
					listner->next = (can_listner_t*)(CANAF_RAM->mask[511-i-( (self->std +1) / 2 + self->std_grp)]);
					CANAF_RAM->mask[511-i-( (self->std +1) / 2 + self->std_grp)] = (uint32_t)(listner);
					ENV_DEBUG("New entry to listner Queue 0\r\n");
					break;
				}
				else {
					ENV_DEBUG("Cant't queue listners for non 'can open' listners");
  				return CAN_ERROR;
				}
			}
				
  	}
  	
  	if ( i == self->ext ) { //The ID mask is not set yet
  		
  		ENV_DEBUG("New EXT entry to Acc filter\r\n");
  		
  		
  		
  		
			
			env_send(CANAF_RAM->mask[511-i-( (self->std +1) / 2 + self->std_grp)]);
			ENV_DEBUG("\r\n");
			
			// update the register pointers if a new row is filled
			can_push_filter(self,CAN_EXT);
			
			
			CANAF_RAM->mask[CANAF->EFF_sa / 4 + i] = (listner->id & 0x1FFFFFFF);
			CANAF_RAM->mask[511-i-( (self->std +1) / 2 + self->std_grp)] = (uint32_t)(listner);
			
			env_send(CANAF_RAM->mask[0]);
			ENV_DEBUG(" Filter ID\r\n");
			env_send((uint32_t)(listner));
			ENV_DEBUG("\r\n");
			ENV_DEBUG("Add rows\r\n");
			

  		
  		self->ext++;
  	}
  
  }		
  	
  /*
  temp = CANAF->ENDofTable / 4 + self->full_can *3 ;
  
  if (temp >= 507 || self->full_can >= 64)
  {
    return 1;
  }
  else
  {
    
    
    
    
    
  }
  */
  
  
 // if ( can_listner->msg->format == STANDARD )
  
  /* Turn on filter */
  CANAF->AFMR = 0x00; //(1<<1);
  return 0;
}

/* ************************************************************************** */

/**
 * \brief keep_alive disable the MOSFET Drivers
 * 
 * Calls it self and polls the CAN RX traffic. Disable MOSFET drivers if no traffic.
 */
env_result_t can_keep_alive( can_t *self, void *none ) 
{
	uint32_t temp = 0;
	if ( can.poll == 0 ) {
		mode = 2;
		TT_SYNC(&mcpwm, mcpwm_enable,TT_ARGS_NONE);
		TT_SYNC(&mcpwm, mcpwm_set_duty, &temp);
		
	}
	
	can.poll = 0;
	TT_WITHIN( 10000000, 100000, self, can_keep_alive, TT_ARGS_NONE);
	
	return 0;
}

/* ************************************************************************** */

/**
 * \brief can_sync responds on group ID
 * 
 * Sends a response with its on ID 
 */
env_result_t can_sync( can_t *self, void *none ) 
{
	
	can_msg_t temp;
	
	ENV_DEBUG("CAN SYNC\r\n");
	
	temp.id = self->ID; 					
 	temp.data[0] = 0; 		
  temp.data[1] = 0;
  temp.len = 0; 			
  temp.format = 1; 					
  temp.type = 0; 	
  
  can_send(self, &temp);
	
	return 0;
}

/* ************************************************************************** */

/**
 * \brief CAN_IRQHandler Handels the traffic on the buss
 * 
 * Calls a specific listner for the corresponding ID and index
 */
void CAN_IRQHandler(void) 
{
  can_msg_t rx_msg;
  uint32_t temp, temp2;
  
  tt_interrupt_mark();
    
  temp = CAN1->ICR;
  
  can_listner_t *listner;
  
  //ENV_DEBUG("CAN_HANDLER()\r\n");
  
  //env_send(temp);
	//ENV_DEBUG(" ICR\r\n");
	//check status of Receive Buffer
 	
	
	temp = CAN1->GSR;
	//env_send(temp);
	//ENV_DEBUG(" GSR\r\n");
	
	if ( temp & (1<<6) )
	{
		CAN1->MOD = 1; // Enter Reset Mode
		CAN1->IER = 0; // Disable All CAN Interrupts
		CAN1->GSR = 0;
	
		/* Request command to release Rx, Tx buffer and clear data overrun */
		//CANx->CMR = CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO;
		CAN1->CMR = (1<<1)|(1<<2)|(1<<3);
		/* Read to clear interrupt pending in interrupt capture register */
		CAN1->ICR;
		CAN1->EWL = 0x60;
		//CAN1->MOD = 0;// Return Normal operating
		CAN1->IER = 0; 	/* Enable CAN Error and Rx Interrupt */
	
		//Reset CANAF value
		CANAF->AFMR = 0x01;
		TT_WITHIN( 100000000, 100000, &can, can_enable, TT_ARGS_NONE);
		//ENV_DEBUG("CAN ERROR!!\r\n");
	}
	
	if(temp & (1<<0))
	{
		temp = (CAN1->RFS &0x03FF);
		//env_send(temp);
		//ENV_DEBUG("\r\n");
		listner = (can_listner_t *)(CANAF_RAM->mask[511-temp]);
		
		temp2 = CAN1->RFS;
		rx_msg.format   = (uint8_t)((temp2 & (1<<31))>>31);
		rx_msg.type     = (uint8_t)((temp2 & (1<<30))>>30);
		rx_msg.len      = (uint8_t)((temp2 & (0x0F<<16))>>16);
		
		
			// Read first 4 data bytes 
			rx_msg.data[0] = CAN1->RDA;
		//env_send(rx_msg.data[0]);
		//ENV_DEBUG("RX\r\n");
			// Read second 4 data bytes 
			rx_msg.data[1] = CAN1->RDB;
			//env_send(rx_msg.data[1]);
		//ENV_DEBUG("RX\r\n");
		
		while( listner != NULL ) {
			//ENV_DEBUG("Listner\r\n");
			if ( listner->index )  {
				
					//ENV_DEBUG("CANopen \r\n");

					if ( (listner->index & listner->mask) == (rx_msg.data[0] & listner->mask))
					{	
						//Save the content of the CAN message in the listner object so it dosent get overwritten before it is handled.
						//listner->msg = rx_msg;
						
						TT_ASYNC( listner->object, listner->method, &rx_msg.data );
						break;	// maybe I dont want to break here?
					
					}
				
				}
			else {
				//ENV_DEBUG("ID found\r\n");
				
				//listner->msg = rx_msg;
			
				TT_ASYNC( listner->object, listner->method,  &rx_msg.data);
				break;	// maybe I dont want to break here?
			}
			
			listner = listner->next;
		}
		
		if ( listner == NULL ) {
			ENV_DEBUG("CAN Error! Cant find requested listner index");
		}
		
		CAN1->CMR = (1<<2);
		can.Rx++;
		can.poll++;
	}
	
	
  tt_schedule(1);
}
